#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os
import re
import sys
import time
import logging
import netifaces
import importlib
import threading
import subprocess
import socketserver
from find_device import get_cpu_serial_number
import ros_robot_controller_sdk as rrc
from ros_robot_controller_send import *

path = os.path.split(os.path.realpath(__file__))[0]
log_file_path = os.path.join(path, "wifi.log")

# 如果日志文件不存在则创建
if not os.path.exists(log_file_path):
    os.system(f'touch {log_file_path}')

# 配置文件路径
config_file_name = "wifi_conf.py"
internal_config_file_dir_path = "/etc/wifi"
external_config_file_dir_path = path
internal_config_file_path = os.path.join(internal_config_file_dir_path, config_file_name)
external_config_file_path = os.path.join(external_config_file_dir_path, config_file_name)



# --- 2. 全局变量 ---
ros_control_enabled = True  # 是否由ROS2控制LED。此标志现在由后台线程动态维护。
current_wifi_mode = None     # 1: AP模式, 2: 客户端模式
server = None                # TCP服务器实例
ip = None                    # 当前设备IP
LOGGING_ENABLED = False       # 日志总开关，默认为关

# --- 3. 日志配置 ---
logger = logging.getLogger("WiFi工具")

def setup_logging():
    """根据全局变量 LOGGING_ENABLED 的值来配置日志系统。"""
    global LOGGING_ENABLED
    if LOGGING_ENABLED:
        handler = logging.FileHandler(log_file_path, 'a', 'utf-8')
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        handler.setFormatter(formatter)
        if logger.hasHandlers():
            logger.handlers.clear()
        logger.addHandler(handler)
        logger.setLevel(logging.INFO)
        logger.propagate = False 
    else:
        if logger.hasHandlers():
            logger.handlers.clear()
        logger.addHandler(logging.NullHandler())
        logger.propagate = False


# --- 4. 核心功能函数 ---
##### MACHINE TYEP SET ######
def set_machine_ros():
    ros_machine_command = "ros2 service call /ros_robot_controller/set_machine_type std_srvs/srv/Trigger '{}'"
    full_command = [
        'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
        '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_machine_command}"
    ]
    result = subprocess.run(full_command, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE)

def get_machine_type_from_config(config_path):
    try:
        with open(config_path, 'r') as f:
            for line in f:
                line = line.strip()
                match = re.match(r'^\s*export\s+MACHINE_TYPE\s*=\s*["\']?(.*?)["\']?\s*$', line)
                if match:
                    return match.group(1)
            raise ValueError(f"MACHINE_TYPE IS None")
    except FileNotFoundError:
        raise FileNotFoundError(f"CONFIG PATH IS ERROR")


def send_machine_type(board,machine_type):
    if 'Tank' in machine_type:
        motor_type = 0x01
        battery_level = 0x1af4
        #battery_level = 0x1af4
    elif 'Acker' in machine_type or 'Mecanum' in machine_type:
        motor_type = 0x02
        battery_level = 0x1af4
    else:
        motor_type = 0x09
        battery_level = 0x1af4
    if motor_type is not None:
        for i in range(2):
            '''
            MOTOR_TYPE_JGB520 0x00
            MOTOR_TYPE_JGB37  0x01
            MOTOR_TYPE_JGB27  0x02
            MOTOR_TYPE_JGB528 0x03
            '''
            board.set_motor_type(motor_type)
            board.set_battery_level(battery_level)
    else:
        print('Please Set the machine_type')

def set_machine_serial(board):
    config_path = "/home/pi/docker/tmp/.typerc"
    machine_type = get_machine_type_from_config(config_path)
    send_machine_type(board,machine_type)

###################################


def update_globals(module_name):
    """动态加载或重载配置模块，并更新全局变量。"""
    try:
        module_spec = importlib.util.find_spec(module_name)
        if module_spec:
            if module_name in sys.modules:
                mdl = importlib.reload(sys.modules[module_name])
            else:
                mdl = importlib.import_module(module_name)
            names = getattr(mdl, '__all__', [name for name in dir(mdl) if not name.startswith('_')])
            globals().update({k: getattr(mdl, k) for k in names})
            logger.info(f"成功加载模块 '{module_name}' 的配置。")
    except Exception as e:
        logger.error(f"加载模块 '{module_name}' 时出错: {e}")

def get_wifi_ip():
    """快速获取wlan0接口的IP地址。"""
    try:
        addresses = netifaces.ifaddresses('wlan0')
        if netifaces.AF_INET in addresses:
            return addresses[netifaces.AF_INET][0]['addr']
        return None
    except (ValueError, KeyError):
        return None
    except Exception as e:
        logger.error(f"获取WiFi IP地址时出错: {e}")
        return None

def reset_wifi():
    """【WiFi重置核心逻辑】"""
    global server
    try:
        logger.info("执行WiFi重置流程...")
        server = stop_server(server)
        os.system("sudo rm /etc/wifi/* -rf > /dev/null 2>&1")
        os.system("sudo touch /tmp/force_ap_mode.flag")
        logger.info("WiFi配置已清除，AP模式标志已创建，将立即重启服务。")
        os.system("sudo systemctl restart wifi.service > /dev/null 2>&1")
    except Exception as e:
        logger.error(f"重置WiFi时出错: {e}")

class TCPHandler(socketserver.BaseRequestHandler):
    """TCP请求处理器。"""
    def handle(self):
        self.request.settimeout(2.0)
        try:
            data = self.request.recv(1024).strip()
            if data == b"resetwifi":
                logger.info("收到 'resetwifi' 命令，立即开始处理...")
                threading.Thread(target=reset_wifi, daemon=True).start()
        except socket.timeout:
            pass
        except Exception as e:
            logger.error(f"TCP处理时出错: {e}")
        finally:
            self.request.close()

class PhoneServer(socketserver.TCPServer):
    """定制的TCP服务器，允许地址重用。"""
    allow_reuse_address = True

def start_server(ip, port):
    """启动TCP服务器。"""
    global server
    if server:
        logger.warning(f"服务器已在监听 {server.server_address}，将先停止旧服务器。")
        stop_server(server)
        server = None
        time.sleep(0.5)
    try:
        server = PhoneServer((ip, port), TCPHandler)
        thread = threading.Thread(target=server.serve_forever, daemon=True)
        thread.start()
        logger.info(f"TCP服务器已启动，监听地址: {ip}:{port}")
        return server
    except Exception as e:
        logger.error(f"启动TCP服务器失败: {e}")
        return None

def stop_server(server_instance):
    """平稳地停止TCP服务器。"""
    if server_instance:
        logger.info(f"正在停止TCP服务器...")
        server_instance.shutdown()
        server_instance.server_close()
        logger.info("TCP服务器已停止。")
    return None

def check_ros2_control():
    """检查ROS2节点状态并更新全局标志。"""
    global ros_control_enabled
    previous_status = ros_control_enabled
    current_status = False

    ros_control_enabled = check_ros_status()
    logger.info(f"当前ROS的状态是：：：：{ros_control_enabled}")


    # try:
    #     process = subprocess.run(
    #         ["docker", "exec", '-u', 'ubuntu', 'MentorPi', '/bin/zsh', '-c', 
    #          'source /home/ubuntu/.zshrc && ros2 topic info /ros_robot_controller/set_led'],
    #         capture_output=True, text=True, timeout=8.0
    #     )
    #     output = process.stdout + process.stderr
    #     if "Subscription count:" in output:
    #         count_str = output.split("Subscription count:")[1].strip().split('\n')[0]
    #         if int(count_str) > 0:
    #             current_status = True
    # except (subprocess.TimeoutExpired, ValueError, IndexError, FileNotFoundError):
    #     pass
    # except Exception as e:
    #     logger.error(f"检测ROS2节点时发生意外错误: {e}")
    # if current_status != previous_status:
    #     ros_control_enabled = current_status
    #     if current_status:
    #         logger.info("【状态更新】检测到ROS2节点已激活，控制权切换到 [ROS2]。")
    #     else:
    #         logger.info("【状态更新】检测到ROS2节点未激活，控制权切换到 [串口]。")


def ros_status_monitor():
    """后台监控线程的主函数，周期性地更新ROS2的状态。"""
    logger.info("后台ROS2状态监控线程已启动。")
    #while True:
    check_ros2_control()
        # 每2秒检查一次，以较快地响应网络和ROS2服务的状态变化
    #time.sleep(2)

def set_led(led_id, on_time, off_time, repeat=0):
    """【统一LED设置接口】根据 ros_control_enabled 标志选择控制方式。"""
    try:
        if ros_control_enabled:
            # logger.info(f"通过 [ROS2] 设置LED: id={led_id}, on={on_time}, off={off_time}")
            # ros_command = (
            #     f"ros2 topic pub --once /ros_robot_controller/set_led ros_robot_controller_msgs/msg/LedState "
            #     f"'{{id: {led_id}, on_time: {on_time}, off_time: {off_time}, repeat: {repeat}}}'"
            # )
            # subprocess.Popen(
            #     ['docker', 'exec', '-u', 'ubuntu', 'MentorPi', '/bin/zsh', '-c', 
            #      f"source /home/ubuntu/.zshrc && {ros_command}"],
            #     stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL
            # )
            set_machine_ros()
            logger.info(f"通过 [ROS2] 设置LED: id={led_id}, on={on_time}, off={off_time}, repeat={repeat}")

            ros_msg = f"{{id: {led_id}, on_time: {on_time}, off_time: {off_time}, repeat: {repeat}}}"
            ros_command = (
                f"ros2 topic pub --once /ros_robot_controller/set_led "
                f"ros_robot_controller_msgs/msg/LedState '{ros_msg}'"
            )

            docker_command = [
                'docker', 'exec', '-u', 'ubuntu', '-w', '/home/ubuntu', 'MentorPi',
                '/bin/zsh', '-c', f"source /home/ubuntu/.zshrc && {ros_command}"
            ]
            result = subprocess.run(docker_command, capture_output=True, text=True, timeout=5)

        else:
            logger.info(f"通过 [串口] 设置LED: id={led_id}, on={on_time}, off={off_time}")
            board = rrc.Board()          # (用于串口控制)
            board.set_led(on_time, off_time, repeat=repeat, led_id=led_id)
            set_machine_serial(board)

    except Exception as e:
        logger.error(f"设置LED时出错: {e}")

def get_active_wifi_connection():
    """快速获取当前活动的WiFi连接名称。"""
    try:
        result = subprocess.run(['nmcli', '-t', '-f', 'NAME,TYPE', 'con', 'show', '--active'], 
                                capture_output=True, text=True, check=True, timeout=2)
        for conn in result.stdout.strip().split('\n'):
            if 'wireless' in conn:
                return conn.split(':')[0]
        return None
    except (subprocess.CalledProcessError, subprocess.TimeoutExpired, FileNotFoundError):
        return None

def disconnect_all_wifi():
    """断开并删除所有已知的WiFi连接。"""
    active_conn = get_active_wifi_connection()
    if active_conn:
        logger.info(f"正在断开并删除旧连接: {active_conn}")
        os.system(f'nmcli connection down "{active_conn}" > /dev/null 2>&1')
        os.system(f'nmcli connection delete "{active_conn}" > /dev/null 2>&1')
        os.system('rm -f /etc/NetworkManager/system-connections/* > /dev/null 2>&1')

def setup_ap_mode():
    """【逻辑优化】配置并启动AP模式。"""
    global current_wifi_mode, ip, ros_control_enabled
    
    logger.info("正在切换到AP（热点）模式...")
    current_wifi_mode = 1
    

    set_led(1, 1, 0, 1)
    set_led(2, 0.5, 0.5, 0)

    disconnect_all_wifi()
    
    try:
        os.system(f'nmcli con add type wifi ifname wlan0 con-name "{WIFI_AP_SSID}" autoconnect yes ssid "{WIFI_AP_SSID}" mode ap 802-11-wireless.band {WIFI_FREQ_BAND} 802-11-wireless.channel {WIFI_CHANNEL} ipv4.method shared ipv4.addresses {WIFI_AP_GATEWAY}/24 wifi-sec.key-mgmt wpa-psk wifi-sec.psk "{WIFI_AP_PASSWORD}" > /dev/null 2>&1')
        os.system(f'nmcli con up "{WIFI_AP_SSID}" > /dev/null 2>&1')
        logger.info(f"AP配置已提交: SSID={WIFI_AP_SSID}")

        for _ in range(16):
            if get_active_wifi_connection() == WIFI_AP_SSID:
                logger.info(f"AP模式启动成功: {WIFI_AP_SSID}")
                ip = WIFI_AP_GATEWAY
                start_server(ip, 9028)
                logger.info("AP模式已激活，确认最终LED状态。")
                set_led(1, 1, 0, 1)
                set_led(2, 0.5, 0.5, 0)
                time.sleep(2)
                return
            time.sleep(0.5)

        logger.warning("AP启动超时，尝试重启NetworkManager...")
        os.system('systemctl restart NetworkManager')
        time.sleep(3)
        if get_active_wifi_connection() == WIFI_AP_SSID:
             logger.info(f"重启NetworkManager后AP模式启动成功。")
             ip = WIFI_AP_GATEWAY
             start_server(ip, 9028)
             logger.info("AP模式已激活，确认最终LED状态。")
             set_led(1, 1, 0, 1)
             set_led(2, 0.5, 0.5, 0)
             time.sleep(2)
             return

        logger.error("无法创建AP，系统将在10秒后重启...")
        time.sleep(10)
        os.system('reboot')

    except Exception as e:
        logger.error(f"设置AP模式时发生严重错误: {e}")

def setup_client_mode():
    """【逻辑优化】连接到客户端（STA）模式。"""
    global current_wifi_mode, ip, ros_control_enabled

    logger.info("正在切换到客户端（STA）模式...")
    current_wifi_mode = 2
    

    logger.info("设置LED为'连接中'状态...")
    set_led(1, 1, 0, 1)
    set_led(2, 0.05, 0.05, 0)
    disconnect_all_wifi()

    logger.info(f"尝试连接到WiFi: {WIFI_STA_SSID}...")
    connect_command = ['nmcli', 'dev', 'wifi', 'connect', WIFI_STA_SSID, 'password', WIFI_STA_PASSWORD]
    
    for attempt in range(2):
        try:
            p = subprocess.run(connect_command, capture_output=True, text=True, timeout=15)
            if p.returncode == 0:
                logger.info("WiFi连接请求成功，正在等待IP地址...")
                for _ in range(20):
                    wifi_ip = get_wifi_ip()
                    if wifi_ip:
                        logger.info(f"成功连接到 {WIFI_STA_SSID} 并获取IP: {wifi_ip}")
                        ip = wifi_ip
                        start_server(ip, 9028)
                        logger.info("STA模式连接成功，设置最终LED状态。")
                        set_led(1, 1, 0, 1)
                        set_led(2, 1, 0, 1)
                        time.sleep(2)
                        return
                    time.sleep(0.5)
                logger.warning("已连接但获取IP超时。")
                break
            else:
                logger.warning(f"连接失败 (尝试 {attempt + 1}/2): {p.stderr.strip()}")
                time.sleep(1)
        except subprocess.TimeoutExpired:
            logger.warning(f"连接命令超时 (尝试 {attempt + 1}/2)。")

    logger.error(f"无法连接到 {WIFI_STA_SSID}，将自动切换到AP模式。")
    
    # --- 【关键改动】 ---
    # 在切换到AP模式前，强制等待一段时间，确保后台的ROS状态监控线程能够运行一次。
    # 由于ros_control_enabled标志位未及时更新而导致LED控制方式错误的问题。
    # 等待时间长于 ros_status_monitor 中的休眠时间（2.1秒）。
    logger.info("等待ROS状态更新...")
    time.sleep(1.1) 
    
    setup_ap_mode()

def WIFI_MGR():

    """WiFi管理主逻辑。"""
    time.sleep(1.1)
    check_ros2_control()
    if WIFI_MODE == 1:
        setup_ap_mode()
    elif WIFI_MODE == 2:
        setup_client_mode()
    else:
        logger.error(f"配置的WIFI_MODE值 '{WIFI_MODE}' 无效，将强制进入AP模式。")
        setup_ap_mode()

# --- 5. 程序主入口 ---
if __name__ == "__main__":
    sn = get_cpu_serial_number()
    WIFI_MODE = 2
    WIFI_AP_SSID = f'HW-{sn[0:8]}' if sn else "HW-DEADBEEF"
    WIFI_STA_SSID = "hiwonder"
    WIFI_AP_PASSWORD = "hiwonder"
    WIFI_STA_PASSWORD = "12345678"
    WIFI_AP_GATEWAY = "192.168.149.1"
    WIFI_CHANNEL = 36
    WIFI_FREQ_BAND = 'a'
    WIFI_LED = True
    LOGGING_ENABLED = True
    force_ap_flag_path = "/tmp/force_ap_mode.flag"
    if os.path.exists(force_ap_flag_path):
        print(f"INFO: 检测到强制AP模式标志 {force_ap_flag_path}，本次将进入AP模式。")
        WIFI_MODE = 1
        try:
            os.remove(force_ap_flag_path)
        except OSError as e:
            print(f"ERROR: 删除AP模式标志文件时出错: {e}")

    if os.path.exists(external_config_file_path):
        sys.path.insert(0, external_config_file_dir_path)
        update_globals(os.path.splitext(config_file_name)[0])
    if os.path.exists(internal_config_file_path):
        sys.path.insert(0, internal_config_file_dir_path)
        update_globals(os.path.splitext(config_file_name)[0])
    
    setup_logging()
    # 检查 ROS2 话题是否存在，设置对应的变量值来控制消息的发送
    check_ros2_control()

    logger.info("====== WiFi管理服务启动 ======")
    
    monitor_thread = threading.Thread(target=ros_status_monitor, daemon=True)
    monitor_thread.start()
    
    if WIFI_LED:
        threading.Thread(target=WIFI_MGR, daemon=True).start()

    try:
        while True:
            time.sleep(3600) 
    except KeyboardInterrupt:
        logger.info("程序收到中断信号，正在退出...")
        stop_server(server)
        sys.exit(0)
    except Exception as e:
        logger.error(f"主线程发生未捕获的异常: {e}")
        sys.exit(1)
